#include "data_type.h"
#include "tools.h"
#include <cuda.h>
#include <cuda_runtime.h>
#include <iostream>
#include <opencv2/opencv.hpp>

#define BLOCK_SIZE_X 32
#define BLOCK_SIZE_Y 32

namespace Registration {

__global__ void
estimate_kernel(const float *rotate_matrix, const float *translation_matrix,
                float3 *vertex_map_current, float3 *normal_map_current,
                float *rotate_previous_inv, float *translation_previous,
                const CameraParams camera_params, float3 *vertex_map_previous,
                float3 *normal_map_prevoius, float distance_threshold,
                float angle_threshold, int cols, int rows,
                float *global_buffer);
__device__ float3 transform_icp(float3 target_point, const float *rotate_matrix,
                                const float *translation_matrix);
__device__ float3 rotate(float3 target_point, const float *rotate_matrix);
__device__ float norm_icp(float3 input_vector);
__device__ float get_angle_between_vector(float3 vector_1, float3 vector_2);
__device__ float3 cross(float3 vec_1, float3 vec_2);
__device__ float dot(float3 vec_1, float3 vec_2);
__device__ float distance(PointXyz pt_1, PointXyz pt_2);
template <int SIZE>
static __device__ __forceinline__ void reduce(volatile double *buffer);

__global__ void reduction_kernel(float *global_buffer, const int length,
                                 float *output);

void estimate_step(const float *rotate_matrix, const float *translation_matrix,
                   float3 *vertex_map_current, float3 *normal_map_current,
                   int image_cols, int image_rows, float *rotate_previous_inv,
                   float *translation_previous, CameraParams &camera_params,
                   float3 *vertex_map_previous, float3 *normal_map_previous,
                   float distance_threshold, float angle_threshold, float *A,
                   float *b);

__global__ void FindCorresponces(PointXyz *next_point_cloud, int next_point_num,
                                 int *match_index_array, int3 voxel_size,
                                 VolumeData *voxel_data, int3 offset,
                                 float3 voxel_scale,
                                 int *valid_points_per_block);

__global__ void RemoveSourceCentroid(VolumeData *voxel_data,
                                     PointXyz *output_point_cloud,
                                     float3 centorid_point, int points_num,
                                     int *match_index_array);
__global__ void RemoveTargetCentroid(PointXyz *input_point_cloud,
                                     PointXyz *output_point_cloud,
                                     float3 centorid_point, int points_num,
                                     int *match_index_array);
__global__ void CalcSVDData(PointXyz *source_point_cloud,
                            PointXyz *target_point_cloud,
                            int *match_index_array, int points_num,
                            float *output);
__global__ void CalcSumPerBlockTarget(PointXyz *total_point_cloud,
                                      int *match_index_array,
                                      float3 *threads_sum, int points_num);

__global__ void TransformPointCLoud(PointXyz *input_point_cloud, int points_num,
                                    float *translation_matrix,
                                    float *rotate_matrix);

__global__ void CalcRegistrationError(VolumeData *voxel_data,
                                      PointXyz *target_point_cloud,
                                      int points_num, int *match_point_index,
                                      float *sum_error_per_block);

float3 CalcSumAllBlock(float3 *sum_of_blocks, int blocks_num);

float3 CalcPointCloudGravity(PointXyz *total_point_cloud,
                             int *match_index_array, int next_points_num,
                             int valid_points_num);
float3 CalcSourcePointCloudGravity(VolumeData *voxle_data,
                                   int *match_index_array, int input_points_num,
                                   int valid_point_num);

__global__ void CalcSumPerBlockSource(VolumeData *voxle_data,
                                      int *match_index_array,
                                      int input_points_num, float3 *sum_data);

float CalcICPError(VolumeData *voxle_data, PointXyz *target_point_cloud,
                   int points_num, int valid_points_num,
                   int *match_point_index);
void PointToPointICP(PointXyz *input_point_cloud, int input_points_num,
                     VolumeData *voxel_data, int3 voxel_size, int3 offset,
                     float3 voxel_scale);

__global__ void merge_point_cloud(PointXyz *target_point_cloud,
                                  int target_point_num, VolumeData *voxel_data,
                                  int3 voxel_size, int3 offset,
                                  float3 vaxel_scale);
} // namespace Registration
